#include "control.h"
#include "tim.h"
#include "motor.h"
#include "gy25.h"
#include "pid.h"

//Timers for encoders, which should be configured for encoder mode
//Timer for periodical interrupt, should be configured for update interrupt
TIM_HandleTypeDef* encoder1,*encoder2,*session;

pid_type_def motor_speed_pid;
pid_type_def motor_angle_pid;

int16_t pwm = 0, pwm1 = 0;//pwm是左轮，pwm1是右轮
int16_t Set_Left_Speed = 0;
int16_t Set_Right_Speed = 0;
float spd_l;
float spd_r;//左右轮当前速度

void motor_control_init(TIM_HandleTypeDef* period,TIM_HandleTypeDef* EA,TIM_HandleTypeDef* EB)
{
	encoder1=EA;
	encoder2=EB;
	session=period;

	__HAL_TIM_ENABLE_IT(period,TIM_IT_UPDATE);
	HAL_TIM_Base_Start(period);
	HAL_TIM_PWM_Start(encoder1,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(encoder1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(encoder2,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(encoder2,TIM_CHANNEL_1);

	static const fp32 Motor_Speed_pid[3] = {Motor_Speed_PID_KP, Motor_Speed_PID_KI, Motor_Speed_PID_KD};
	//static const fp32 Motor_Angle_pid[3] = {Motor_Angle_PID_KP, Motor_Angle_PID_KI, Motor_Angle_PID_KD};
	
  	PID_init(&motor_speed_pid, PID_POSITION, Motor_Speed_pid, Motor_Speed_PID_MAX_OUT, Motor_Speed_PID_MAX_IOUT);
	//PID_init(&motor_angle_pid, PID_DELTA, Motor_Angle_pid, Motor_Angle_PID_MAX_OUT, Motor_Angle_PID_MAX_IOUT);
	
	PID_clear(&motor_speed_pid);
	PID_clear(&motor_angle_pid);

}

#ifdef GY25

void Turn_right()
{
	uint16_t Angle_Origin = 0;
	uint16_t Angle_Delat = 0;
	Angle_Origin = Angle;
/* 	HAL_GPIO_WritePin(GPIOA,GPIO_PIN_8,GPIO_PIN_SET);//PA8 high
	HAL_GPIO_WritePin(GPIOC,GPIO_PIN_8,GPIO_PIN_RESET);//PC8 low

	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET);
	HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_SET); */
	Set_Left_Speed = 1;
	Set_Right_Speed = -1;
	while(Angle_Delat<90)
	{
		Angle_Delat = Angle - Angle_Origin;
		printf("Angle_Delat:%d\r\n",Angle_Delat);
	}
	Mot_stop_left();
	Mot_stop_right();
}

#endif

void controlCallback(TIM_HandleTypeDef *htim)
{

  static int time;
  if(htim->Instance == session->Instance)
  {	
		spd_l =(1.0*encoder1->Instance->CNT-32768)/(0.02*500*30*4);
		encoder1->Instance->CNT=32768;
		spd_r =(1.0*encoder2->Instance->CNT-32768)/(0.02*500*30*4);
		encoder2->Instance->CNT=32768;
		
		time++;

		if(time % 10 == 0)
		{

			pwm += PID_calc(&motor_speed_pid,spd_l,Set_Left_Speed);
			pwm1 += PID_calc(&motor_speed_pid,spd_r,Set_Right_Speed);
			//printf("pwm_left:%d\r\n\r\n",pwm);
			printf("pwm_right:%d\r\n\r\n",pwm1);

			if(pwm > 5000)	pwm=5000;
			else if(pwm < -5000)	pwm = -5000;
			if(pwm1 > 5000)	pwm1=5000;
			else if(pwm1 < -5000)	pwm1 = -5000;//限幅pwm
			//Mot_exec_left(pwm);
			//Mot_exec_right(pwm1);
			Set_Left_MotorVoltageAndDirection(pwm);
			Set_Right_MotorVoltageAndDirection(pwm1);
		}
		if(time >= 1000)
		{
			time = 0;
		}
  }
	
	
}
